#!/usr/bin/env python  

#Daniel Cook - 
#COMP4766 - A1

import roslib
roslib.load_manifest('comp4766_a1')
import rospy
import math
# Add other import statements if necessary

# Import the formats needed for used services and messages
from turtlesim.srv import TeleportRelative

def spiral():
    # After teleporting the robot should take a sleep.  If we go too
    # quickly then the teleports happen faster than calls to pose_callback.
    # This means that the pen's colour is not appropriately updated before each
    # teleport (try increasing the rate and you'll see what happens).
    rate = rospy.Rate(26)
    
    rate.sleep()
        
    teleporter(0.44, math.radians(90))
    rate.sleep()
    teleporter(0.44, math.radians(-90))
    rate.sleep()
    
    teleporter(0.44, math.radians(-90))
    rate.sleep()       
    
    w = 1
    while(w < 25):
        for x in range(0, w):
            teleporter(0.44, math.radians(0))
            rate.sleep()
        teleporter(0.44, math.radians(-90))
        rate.sleep()
        
        if w!=24:
            for x in range(0, w):
                teleporter(0.44, math.radians(0))
                rate.sleep()
            teleporter(0.44, math.radians(-90))
            rate.sleep()
        
        w = w+1        

if __name__ == '__main__':
    
    global teleporter

    rospy.init_node('teleporter')
    
    teleporter = rospy.ServiceProxy('/turtle1/teleport_relative', TeleportRelative)

    spiral()

    # Sometimes we need to let let a node stay active so that it can
    # respond to callbacks.  We can do this with:
    #   rospy.spin()
    # However, all of the work is now done so we can just let the node die.
